package co.edu.unal.cg.splines;
import java.util.ArrayList;

import org.la4j.matrix.Matrix;
import org.la4j.matrix.dense.Basic2DMatrix;

import processing.core.PApplet;
import processing.core.PVector;

public class HermiteSpline extends Spline {

	private ArrayList<HermiteSpline.ControlPoint> controlPoints = new ArrayList<HermiteSpline.ControlPoint>();

	public static class ControlPoint {
		public PVector coordinates;
		public PVector orientation;

		public ControlPoint(PVector coordinates, PVector orientation) {
			this.coordinates = coordinates;
			this.orientation = orientation;
		}
	}

	public HermiteSpline(double d) {
		super(d);
	}
	
	public void addControlPoint(ControlPoint point) {
		controlPoints.add(point);
	}

	public void addControlPoint(PVector coordinates, PVector orientation) {
		this.addControlPoint(new ControlPoint(coordinates, orientation));
	}

	public void drawGradients(PApplet canvas, float scale) {
		canvas.pushStyle();
		canvas.stroke(canvas.color(255,255,0));
		for (int i = 0; i < controlPoints.size() - 1; i++) {
			PVector p1 = controlPoints.get(i).coordinates;
			PVector p2 = PVector.add(p1,
					PVector.mult(controlPoints.get(i).orientation, scale));
			canvas.line(p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
		}
		canvas.popStyle();
	}

	@Override
	public ArrayList<Matrix> getCoefficients() {
		ArrayList<Matrix> coefficients = new ArrayList<Matrix>();
		for (int i = 0; i < controlPoints.size() - 1; i++) {
			Matrix C = new Basic2DMatrix(3, 4);

			double c = controlPoints.get(i).orientation.x;
			double d = controlPoints.get(i).coordinates.x;
			double b = 3 * controlPoints.get(i + 1).coordinates.x - 2 * c - 3
					* d - controlPoints.get(i + 1).orientation.x;
			double a = controlPoints.get(i + 1).coordinates.x - b - c - d;

			C.set(0, 0, a);
			C.set(0, 1, b);
			C.set(0, 2, c);
			C.set(0, 3, d);

			c = controlPoints.get(i).orientation.y;
			d = controlPoints.get(i).coordinates.y;
			b = 3 * controlPoints.get(i + 1).coordinates.y - 2 * c - 3 * d
					- controlPoints.get(i + 1).orientation.y;
			a = controlPoints.get(i + 1).coordinates.y - b - c - d;

			C.set(1, 0, a);
			C.set(1, 1, b);
			C.set(1, 2, c);
			C.set(1, 3, d);

			c = controlPoints.get(i).orientation.z;
			d = controlPoints.get(i).coordinates.z;
			b = 3 * controlPoints.get(i + 1).coordinates.z - 2 * c - 3 * d
					- controlPoints.get(i + 1).orientation.z;
			a = controlPoints.get(i + 1).coordinates.z - b - c - d;

			C.set(2, 0, a);
			C.set(2, 1, b);
			C.set(2, 2, c);
			C.set(2, 3, d);

			coefficients.add(i, C);

		}

		return coefficients;

	}

}
